#include "viewer_gui.h"

// ----------------------------------------------------------------------------
// redraw_File
// ----------------------------------------------------------------------------
void main_GUI::minimisar()
{

double eta, eta_n, beta,x[3],y[3],norma,Qp[3],Q[3],v[3],Fx;
double w[3],w_p[3],w_m[3];
double french=6.667, deltaW[3];
double suma, sigma, eta_t,resto,diametre;

int iter,N_iter, j, i, k;
int paso,count;
char char_out[250];
int *UPs, *flag_UPs;
int G1;

double distancia;

	G1 = number_UP;//Guide->GetNumberOfPoints();

    eta = atof(v_eta->value());
    eta_n = atof(v_eta_neighb->value());
    eta_t = eta_n;
	beta = atof(v_beta->value());
	french = atof(v_diameter->value());

	sigma = atof(v_sigma->value());

    N_iter = atof(m_iteration->value());

    //printf("%f %f %f\n",eta,eta_n,beta);

	UPs = (int*) malloc ((G1+2)*sizeof(int));
	flag_UPs = (int*) malloc ((G1+2)*sizeof(int));

	// repere Scanner
	vtkTransform *m_transf = vtkTransform::New();
	m_transf->PostMultiply();
	vtkTransformFilter *m_transfilter = vtkTransformFilter::New();
	m_transfilter->SetInput(Guide);
	m_transfilter->SetTransform(m_transf);
	m_transfilter->Update();

	m_transf->Translate(-centre[0],-centre[1],-centre[2]);//0,0,0
	m_transf->Update();

	m_transf->RotateY(90.0);
	m_transf->Update();
	m_transf->RotateX(90.0);
	m_transf->Update();

	addRodriguez(a_r, a_t);
	m_transf->Concatenate(l_matrix);
	m_transfilter->Update();
	
	distancia = Guide_Dist(m_transfilter);

	W = vtkPoints::New();
	DW = vtkPoints::New();

	// UPs borde W_o
	W->InsertNextPoint(0,0,0);
	// adding point from CL
    //W_1 to W_G1
	for(i=0;i<G1;i++)//for(i=G1-1;i>=0;i--)
    {
		W->InsertNextPoint(m_transfilter->GetOutput()->GetPoint(i));
		DW->InsertNextPoint(0,0,0);
    }

	// UPs borde W_(G1+1)
	W->InsertNextPoint(0,0,0);
	//assigne value
    // UPs borde W_o
    W->GetPoint (1, x);
    W->GetPoint (5, y);
	y[0] = x[0] - y[0];  
    y[1] = x[1] - y[1];  
    y[2] = x[2] - y[2];     
    norma = vtkMath::Norm(y); 
    
	x[0] = x[0] +y[0]/norma;  
	x[1] = x[1] +y[1]/norma;  
	x[2] = x[2] +y[2]/norma;   

	W->SetPoint(0,x[0],x[1],x[2]);
	
	// UPs borde W_(G1+1)
    W->GetPoint(G1, x);
    W->GetPoint(G1-5, y);

    y[0] = x[0] - y[0];  
    y[1] = x[1] - y[1];  
    y[2] = x[2] - y[2];     
    norma = vtkMath::Norm(y); 
    
    x[0] = x[0] + y[0]/norma;  
    x[1] = x[1] + y[1]/norma;  
    x[2] = x[2] + y[2]/norma;     

	W->SetPoint(G1+1,x[0],x[1],x[2]);

 	//retroproyection vector
	vec_retrop = vtkPoints::New();
	vec_retrop->DeepCopy(W);

	vector_retroproj();

	vtkDoubleArray *radios_temp = vtkDoubleArray::New();
	radios_temp->SetName("MaximumInscribedSphereRadius");

	radios_temp->InsertNextTuple1(radios->GetTuple1(0));
	for(i=0;i<G1;i++)
		radios_temp->InsertNextTuple1(radios->GetTuple1(i));
	radios_temp->InsertNextTuple1(radios->GetTuple1(G1-1));
	for(i=number_UP;i<radios->GetNumberOfTuples();i++)
		radios_temp->InsertNextTuple1(radios->GetTuple1(i));


	//adding all structure
	for(i=number_UP;i<Guide->GetNumberOfPoints();i++)
    {
		W->InsertNextPoint(m_transfilter->GetOutput()->GetPoint(i));
		DW->InsertNextPoint(0,0,0);

    }

	iter=0;

	//printf("number %d\n",radios_temp->GetNumberOfTuples());getchar();

	while(iter<N_iter)
    {
		permutar(UPs,G1);// variation en l'activation des UPs
		for(i=0;i<G1;i++)
		{
			j = UPs[i]+1;//UPs [1 to G1]

			W->GetPoint(j,Q);//UPs[0] et UPs[G1+1] sont fixe
			near_point(Q, Qp, v);
			x[0] = Qp[0] - Q[0];  
			x[1] = Qp[1] - Q[1];  
			x[2] = Qp[2] - Q[2];     

			norma = vtkMath::Norm(x);
			diametre = radios_temp->GetComponent(0,j)-french;// astes restaba -1 para sincronizar con el arreglo radios
														// ahora agrege dos radios al inicio y al final del segmento UPs
														// para sincronizar mas facil, el registro de ellos

			if(diametre<0.01) diametre = 0.01;
			if( norma >= diametre )
			{
				Fx = 1.0;
				flag_UPs[j]=1;
			}
			else
			{
				Fx = 0.0;
				flag_UPs[j]=0;
			}

			W->GetPoint(j,w);
			W->GetPoint(j+1,w_p);
			W->GetPoint(j-1,w_m);
			deltaW[0] = eta*x[0]*Fx + beta*(w_m[0]+w_p[0]-2.0*w[0])*Fx;
			deltaW[1] = eta*x[1]*Fx + beta*(w_m[1]+w_p[1]-2.0*w[1])*Fx;
			deltaW[2] = eta*x[2]*Fx + beta*(w_m[2]+w_p[2]-2.0*w[2])*Fx;

			w[0] = w[0] + deltaW[0];
			w[1] = w[1] + deltaW[1];
			w[2] = w[2] + deltaW[2];

			W->SetPoint(j,w);
			DW->SetPoint(j,deltaW);
			
			paso = 1;
			if(j<=1) paso = j;
			if(j>=(G1-1)) paso = G1-j;
			//voissinage
			if(Fx>0.0)
			{
				k=j;
				suma=1;
				while(k<j+paso)
				{
					W->GetPoint(k+1,w);
					w[0] = w[0] + eta_n*x[0]/(suma*suma);
					w[1] = w[1] + eta_n*x[1]/(suma*suma);
					w[2] = w[2] + eta_n*x[2]/(suma*suma);
					W->SetPoint(k+1,w);
					DW->SetPoint(k+1,eta_n*x[0]/(suma*suma),eta_n*x[1]/(suma*suma),eta_n*x[2]/(suma*suma));

					k=k+1;
					suma = suma + 1.0;
				}
				k=j;
				while(k>j-paso+1)
				{
					W->GetPoint(k-1,w);
					w[0] = w[0] + eta_n*x[0]/(suma*suma);
					w[1] = w[1] + eta_n*x[1]/(suma*suma);
					w[2] = w[2] + eta_n*x[2]/(suma*suma);
					W->SetPoint(k-1,w);
					DW->SetPoint(k-1,eta_n*x[0]/(suma*suma),eta_n*x[1]/(suma*suma),eta_n*x[2]/(suma*suma));
					k=k-1;
					suma = suma + 1.0;
				}
			}
		}

		resto = ajuste_UP(distancia);
//getchar();
/*		// UPs borde
		W->GetPoint(G1,w);
		W->GetPoint(G1-15,w_m);
		x[0] = w[0] - w_m[0];  
		x[1] = w[1] - w_m[1];  
		x[2] = w[2] - w_m[2];     
		norma = vtkMath::Norm(x); 

		w[0] = w[0] +x[0]*resto/norma;  
		w[1] = w[1] +x[1]*resto/norma;  
		w[2] = w[2] +x[2]*resto/norma; 
		W->SetPoint(G1+1,w);
*/
		iter = iter + 1;
		eta_t = atof(v_eta_neighb->value())/iter;
		eta_n = eta_t;


		sprintf_s(char_out,"%d",iter);
		//printf("%d\n",iter);
		m_iter->value(char_out);
		Fl::check();

		vtkPoints *Pts_guide3D = vtkPoints::New();
		count=0;
		for(k=0;k<=G1+1;k++)
		{
			W->GetPoint(k,w);
			Pts_guide3D->InsertNextPoint(w[0],w[1],w[2]);
		}

		m_transfilter->GetOutput()->SetPoints(Pts_guide3D);
		project(vtkPolyData::SafeDownCast(m_transfilter->GetOutput()), Guide2D);

		move_sphere(m_transfilter);
//
		Pts_guide3D->Delete();

		//deform_VTK();
		renderWindow->Render();
	}

	vtkCellArray *Lines = vtkCellArray::New();
	Lines->Initialize();
/**/
	Lines->InsertNextCell(G1);
	for(i=G1-1;i>=0;i--)//for(i=0;i<G1;i++)
	{
		Lines->InsertCellPoint(i);
	}

	//adding all structure////
	Lines->InsertNextCell(W->GetNumberOfPoints()-G1-2);
	for(i=W->GetNumberOfPoints()-1;i>=G1+2;i--)
	{
		Lines->InsertCellPoint(i);
	}


	GuideDef->SetPoints(W);
	GuideDef->SetLines(Lines);
	GuideDef->GetPointData()->AddArray(radios_temp);
	//GuideDef->GetPointData()->AddArray(EdgeArray);
	//GuideDef->GetPointData()->AddArray(EdgeCoord);

	Lines->Delete();

	free(flag_UPs);
	free(UPs);

}

// ----------------------------------------------------------------------------
// ----------------------------------------------------------------------------
void main_GUI::vector_retroproj()
{
int i;
double v[3],norma;

    for(i=0;i<vec_retrop->GetNumberOfPoints();i++)
    {
        Lunderquist2D->GetPoint(i,v);
        norma = vtkMath::Norm(v);
		v[0] = v[0]/norma;
		v[1] = v[1]/norma;
		v[2] = v[2]/norma;
        vec_retrop->SetPoint(i,v);  
    }
}

// ----------------------------------------------------------------------------
// ----------------------------------------------------------------------------
void main_GUI::permutar(int *tab, int n)
{
int i,j,a,trouve=1;

    for(i=0;i<n;i++)// inicializacion
        tab[i]=-1;

    for(i=0;i<n;i++)
    {
        trouve=1;
		while(trouve)
        {
            trouve=0;
            a=rand()%n;
            for(j=0;j<n;j++)
                if(tab[j]==a) trouve=1;
            if(trouve==0) tab[i]=a;
        }
        
    }
}

// ----------------------------------------------------------------------------
// ----------------------------------------------------------------------------
void main_GUI::near_point(double *Q, double *Qp, double *v)
{
// Reference : http://www.mathematics-online.org/kurse/kurs8/seite33.html

int i,indice=0;  
double t,dist,min=1000.0,x[3],P[3],PQ[3];
int G2D;

	G2D = vec_retrop->GetNumberOfPoints();

    for(i=0;i<G2D;i++)
    {
        vec_retrop->GetPoint(i,P);
		PQ[0] = Q[0];  
        PQ[1] = Q[1];  
        PQ[2] = Q[2];     

        vtkMath::Cross(PQ,P,x);
        dist = vtkMath::Norm(x);
        
        if(dist<min)
        {
            min = dist;
            indice = i;       
        }
    }
    vec_retrop->GetPoint(indice,v);
    PQ[0] = Q[0];  
    PQ[1] = Q[1];  
    PQ[2] = Q[2];     
    
    t = vtkMath::Dot(PQ,v);

	Qp[0] = t*v[0];  
    Qp[1] = t*v[1];  
    Qp[2] = t*v[2];  
	//printf("dist %2.3f ",min);

}



// ----------------------------------------------------------------------------
// ----------------------------------------------------------------------------
double main_GUI::ajuste_UP(double distancia)
{
int i;
double p[3], restante,dist=0,min=1000,max=0;
double w[3],w_p[3];

int G1 = Guide->GetNumberOfPoints();


for(i=0;i<G1;i++)
{
	
	W->GetPoint(i,w);
	W->GetPoint(i+1,w_p);

	p[0] = w[0] - w_p[0];
	p[1] = w[1] - w_p[1];
	p[2] = w[2] - w_p[2];
	dist = dist + vtkMath::Norm(p);
	if(vtkMath::Norm(p)<min)
		min =vtkMath::Norm(p);
	if(vtkMath::Norm(p)>max)
		max =vtkMath::Norm(p);
}

	restante = distancia - dist;

	//printf("restante %f\n",restante);

	return restante;
}


// ----------------------------------------------------------------------------
// ----------------------------------------------------------------------------
double main_GUI::Guide_Dist(vtkTransformFilter *m_transfilter)
{
double w[3],w_p[3],p[3];
int i;
int G1 = m_transfilter->GetOutput()->GetNumberOfPoints();
double distancia =0;

	for(i=0;i<G1-1;i++)
	{
		m_transfilter->GetOutput()->GetPoint(i,w);
		m_transfilter->GetOutput()->GetPoint(i+1,w_p);

		p[0] = w[0]-w_p[0];
		p[1] = w[1]-w_p[1];
		p[2] = w[2]-w_p[2];
		distancia = distancia +vtkMath::Norm(p);
	}
	return(distancia);
}

// ----------------------------------------------------------------------------
// ----------------------------------------------------------------------------
void main_GUI::m_cal_error()
{

	double lineP0[3];
	double lineP1[3];
 
	double p0[3];
	double dist0, min,error = 0, max,mean, std;

	int m,n,i,j;
	char char_out[250];

	n = Lunderquist2D->GetNumberOfPoints();

	m = Guide2D->GetNumberOfPoints();

	vtkDoubleArray* dist = vtkDoubleArray::New( );
	dist->SetName( "ErrorDistance" );
	dist->SetNumberOfComponents( 1 );
	//dist->SetNumberOfTuples(m);
	
	max = 0;
	for(i=0;i<m;i++)
	{
		Guide2D->GetPoint(i,p0);
		if(fabs(p0[0])<255.0*0.6 && fabs(p0[1])<255.0*0.6)//dans l'image
		{
			min = 10000.0;
			for(j=0;j<n-1;j++)
			{
				Lunderquist2D->GetPoint(j,lineP0);
				Lunderquist2D->GetPoint(j+1,lineP1);
		
				dist0 = vtkLine::DistanceToLine(p0, lineP0, lineP1);
				//printf("dist %3.2f ",dist0);////[%3.2f,%3.2f,%3.2f,][%3.2f,%3.2f,%3.2f,][%3.2f,%3.2f,%3.2f,]
				if(dist0<min) min = dist0;
			}
			error = error + min;
			dist->InsertNextTuple1(min);
			//printf("dist %3.2f %3.2f",min,dist->GetTuple1(i));//getchar();
		}

	}
	mean = error/m;
	sprintf_s(char_out,"%7.5f",mean);
	m_err_mean_mm->value(char_out);

	sprintf_s(char_out,"%7.5f",mean/0.6);
	m_err_mean_pixel->value(char_out);



	std = 0;
	max = 0;
	m = dist->GetNumberOfTuples();
	for(i=0;i<m;i++)
	{
		dist0 = dist->GetTuple1(i);
		std = std + (dist0-mean)*(dist0-mean);
		if(dist0>max) max = dist0;
	}
	std = sqrt(std/m);

	sprintf_s(char_out,"%7.5f",std);
	m_err_std_mm->value(char_out);

	sprintf_s(char_out,"%7.5f",std/0.6);
	m_err_std_pixel->value(char_out);

	sprintf_s(char_out,"%7.5f",max);
	m_err_max_mm->value(char_out);

	sprintf_s(char_out,"%7.5f",max/0.6);
	m_err_max_pixel->value(char_out);
	return;
}